/* gm_monitor.c -- Monitora a posição dos objetos em campo
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 *
 * This file is part os GEARMedula.
 * 
 * GEARMedula is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, version 3 of the
 * License.
 * 
 * GEARMedula is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#include <pthread.h>
#include <string.h>

#include "gm_utils.h"
#include "gm_protocol.h"
#include "gm_display.h"

#include "gm_monitor.h"
struct gm_monitor_t
{
    int f_real;

    /* Informações do mundo */
    int world_size;
    float * world;
    pthread_mutex_t world_mutex;

    /* Informações dos objetos em campo */
    int field_size;
    float * field;
    pthread_mutex_t field_mutex;

    int f_display;
    gm_display_t * display;
};


gm_monitor_t *
gm_monitor_new( const int f_real )
{
    gm_monitor_t * self = NULL;
    int i = 0;

    self = XMALLOC( gm_monitor_t, 1);

    /* Iniciando o mundo */
    self->world_size = LENGTH_WORLD_VECTOR;
    self->world = XMALLOC( float, self->world_size );
    for( i = 0 ; i < self->world_size ; i++ )
	self->world[i] = 0;
    pthread_mutex_init( &(self->world_mutex),
			NULL );

    /* Iniciando field */
    self->field_size = LENGTH_FIELD_VECTOR;
    self->field = XMALLOC( float, self->field_size );
    for( i = 0 ; i < self->field_size ; i++ )
	self->field[i] = 0.0;
    pthread_mutex_init( &(self->field_mutex),
			NULL );
    
    if( f_real )
      self->f_real = 1;
    else
    {
	self->f_real = 0;
    }

    self->f_display = 0;
    self->display = NULL;
    
    return self;
}

void
gm_monitor_destroy( gm_monitor_t * self )
{
    XFREE( self->field );
    XFREE( self->world );

    if( self->f_display )
	gm_monitor_display_enable( self, 0 );
    
    XFREE( self );
}

void
gm_monitor_display_enable( gm_monitor_t * self, const int enable )
{
    if( enable )
    {
	self->f_display = 1;
	self->display = gm_display_new( self );
	gm_display_init( self->display );
    }
    else
    {
	self->f_display = 0;
	if( self->display != NULL )
	    gm_display_destroy(self->display);
	self->display = NULL;
    }
}

void
gm_monitor_set_world( gm_monitor_t * self,
		      const float * world_vec )
{
	pthread_mutex_lock( &(self->world_mutex) );

	/* Copiando todo o vetor de informações */
	if( (memcpy(self->world,
		    world_vec,
		    sizeof(float) * self->world_size)) == NULL )
	    xerror_error("Nao foi possível copiar o conteudo dos vetores "
			 "de informacao");

	pthread_mutex_unlock( &(self->world_mutex) );
}


void
gm_monitor_get_world( gm_monitor_t * self,
		      float * world )
{
    float buff[LENGTH_WORLD_VECTOR];
    
    if( !(self->f_real) )
    {
	gm_sim_get_world( buff );
	gm_monitor_set_world( self, buff );
    }

    pthread_mutex_lock( &(self->world_mutex) );

    /* Copiando todo o vetor de informações */
    if( (memcpy(world,
		self->world,
		sizeof(float) * self->world_size)) == NULL )
	xerror_error("Nao foi possível copiar o conteudo dos vetores "
		     "de informacao");

    pthread_mutex_unlock( &(self->world_mutex) );
}


void
gm_monitor_set_field( gm_monitor_t * self,
		      const float * field_vec )
{    
    /* Atualizando o próprio campo */
    pthread_mutex_lock( &(self->field_mutex) );

    /* Copiando todo o vetor de informações */
    if( (memcpy(self->field,
		field_vec,
		sizeof(float) * self->field_size)) == NULL )
	xerror_error("Nao foi possível copiar o conteudo dos vetores "
		     "de informacao");

    pthread_mutex_unlock( &(self->field_mutex) );

}

void
gm_monitor_get_field( gm_monitor_t * self, float * field )
{
    float buff[LENGTH_FIELD_VECTOR];
    
    if( !(self->f_real) )
    {
	gm_sim_get_field( buff );
	gm_monitor_set_field( self, buff );
    }

    /* Retirando informações do próprio campo */
    pthread_mutex_lock( &(self->field_mutex) );

    /* Copiando todo o vetor de informações */
    if( (memcpy(field,
		self->field,
		sizeof(float) * self->field_size)) == NULL )
	xerror_error("Nao foi possível copiar o conteudo dos vetores "
		     "de informacao");

    pthread_mutex_unlock( &(self->field_mutex) );    
}
